# -*- coding: utf-8 -*-
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Image
import tf2_ros
import tf2_geometry_msgs

# 定义无人机数量
uav_id = 0  # 假设你有4架无人机，根据需要调整
pose_publishers = None
def odometry_callback(msg):
    # 创建 PoseStamped 消息
    pose_stamped = PoseStamped()
    pose_stamped.header = msg.header
    pose_stamped.pose = msg.pose.pose

    # 发布 PoseStamped 消息到对应的无人机话题
    pose_publishers.publish(pose_stamped)


if __name__ == '__main__':
    rospy.init_node('odometry_to_posestamped_'+str(uav_id))


    # 为每架无人机初始化 PoseStamped 发布器

    topic_name = 'iris_{}/lidar/trans_posestamped'.format(uav_id)
    pose_publishers = rospy.Publisher(topic_name, PoseStamped, queue_size=10)

    # 订阅每架无人机的 Odometry 话题

    topic_name = 'iris_{}/Odometry'.format(uav_id)
    rospy.Subscriber(topic_name, Odometry, odometry_callback)

    rospy.spin()

